#include "VideoFrameObservable.h"


VideoFrameObservable::VideoFrameObservable(IplImage* _frame)
{
	setFrame(_frame);

	m_cursor_x = m_height/(2*BLOCK_SIZE);
	m_cursor_y = 3*m_width/(4*BLOCK_SIZE);

	m_pointRed = 0;
	m_pointGreen = 0;
	m_pointBlue = 255;
	m_calibrated = false;

	m_YCrCb = new uchar[(m_height/BLOCK_SIZE)*(m_width/BLOCK_SIZE)*3];

	m_virtCur = new HandFinder(m_YCrCb,_frame->width,_frame->height,1,0.5,0,0,0,30);
}

void VideoFrameObservable::setFrame(IplImage* _frame)
{
	m_width     = _frame->width;
	m_height    = _frame->height;
	m_nchannels = _frame->nChannels;
	m_step      = _frame->widthStep;

	m_data = ( uchar* ) _frame->imageData;

	_frame->height /= BLOCK_SIZE;
	_frame->width /= BLOCK_SIZE;
}

void VideoFrameObservable::calibrate()
{
	m_fingerY =  get_pointYCbCr(0, m_cursor_x,m_cursor_y);
	m_fingerCb = get_pointYCbCr(1, m_cursor_x,m_cursor_y);
	m_fingerCr = get_pointYCbCr(2, m_cursor_x,m_cursor_y);

	m_virtCur->setColor(m_fingerY, m_fingerCb, m_fingerCr);

	m_pointRed = 255;
	m_pointGreen = 0;
	m_pointBlue = 0;
	m_calibrated = true;
}

bool VideoFrameObservable::isCalibrated()
{
	return m_calibrated;
}

uchar VideoFrameObservable::get_fY()
{
	return m_fingerY;
}
uchar VideoFrameObservable::get_fCb()
{
	return m_fingerCb;
}
uchar VideoFrameObservable::get_fCr()
{
	return m_fingerCr;
}
double fullquantable[BLOCK_SIZE][BLOCK_SIZE]={
		{ 50, 127, 127, 127, 127, 127, 127, 127},
		{ 127, 127, 127, 127, 127, 127, 127, 127},
		{ 127, 127, 127, 127, 127, 127, 127, 127},
		{ 127, 127, 127, 127, 127, 127, 127, 127},
		{ 127, 127, 127, 127, 127, 127, 127, 127},
		{ 127, 127, 127, 127, 127, 127, 127, 127},
		{ 127, 127, 127, 127, 127, 127, 127, 127},
		{ 127, 127, 127, 127, 127, 127, 127, 127}};


double VideoFrameObservable::get_pointYCbCr(int _channel, int _x, int _y)
{
    uchar B = m_data[_x*m_step + _y*m_nchannels + 0];
    uchar G = m_data[_x*m_step + _y*m_nchannels + 1];
    uchar R = m_data[_x*m_step + _y*m_nchannels + 2];
    if (_channel == 0)
    	return  0.299*R + 0.587*G + 0.114*B;
    else if (_channel == 1)
    	return -0.1687*R - 0.3313*G + 0.5*B + 128;
    else
    	return  0.5*R - 0.4187*G - 0.0813*B + 128;
}

void VideoFrameObservable::set_pointBGR(int channel, int x, int y, double val)
{
	m_data[x * m_step + y * m_nchannels + (channel)] = val;
}


void VideoFrameObservable::d1idct(double data[BLOCK_SIZE])
{
	double temp1[BLOCK_SIZE];
	double costab[7]={0.3535534,0.5411962,1.306567,0.5097956,
						0.6013452,0.8999777,2.562936};

	temp1[0]=0.3535554*data[0];
	temp1[1]=costab[0]*data[4];
	temp1[2]=0.5*data[2];
	temp1[3]=costab[0]*(data[6]+data[2]);
	temp1[4]=0.5*data[1];
	temp1[5]=costab[0]*(data[5]+data[3]);
	temp1[6]=0.5*(data[1]+data[3]);
	temp1[7]=costab[0]*(data[5]+data[7]+data[1]+data[3]);

	data[0]=(double)(temp1[0]+temp1[1]);
	data[1]=(double)(temp1[0]-temp1[1]);
	data[2]=(double)(costab[1]*(temp1[2]+temp1[3]));
	data[3]=(double)(costab[2]*(temp1[2]-temp1[3]));
	data[4]=(double)(temp1[4]+temp1[5]);
	data[5]=(double)(temp1[4]-temp1[5]);
	data[6]=(double)(costab[1]*(temp1[6]+temp1[7]));
	data[7]=(double)(costab[2]*(temp1[6]-temp1[7]));

	temp1[0]=data[0]+data[2];
	temp1[1]=data[1]+data[3];
	temp1[2]=data[0]-data[2];
	temp1[3]=data[1]-data[3];
	temp1[4]=costab[3]*(data[4]+data[6]);
	temp1[5]=costab[4]*(data[5]+data[7]);
	temp1[6]=costab[6]*(data[4]-data[6]);
	temp1[7]=costab[5]*(data[5]-data[7]);

	data[0]=(double)((temp1[0]+temp1[4]));
	data[1]=(double)((temp1[1]+temp1[5]));
	data[3]=(double)((temp1[2]+temp1[6]));
	data[2]=(double)((temp1[3]+temp1[7]));
	data[7]=(double)((temp1[0]-temp1[4]));
	data[6]=(double)((temp1[1]-temp1[5]));
	data[4]=(double)((temp1[2]-temp1[6]));
	data[5]=(double)((temp1[3]-temp1[7]));
}

void VideoFrameObservable::d1dct(double data[BLOCK_SIZE])
{
	double temp1[BLOCK_SIZE];
	double costab[7]={0.7071062,1.8477588,0.7653644,0.9807853,
						0.8314692,0.5555693,0.1950888};
	temp1[0]=0.5*(data[0]+data[7]);
	temp1[1]=0.5*(data[1]+data[6]);
	temp1[2]=0.5*(data[3]+data[4]);
	temp1[3]=0.5*(data[2]+data[5]);
	temp1[4]=costab[3]*(data[0]-data[7]);
	temp1[5]=costab[4]*(data[1]-data[6]);
	temp1[6]=costab[6]*(data[3]-data[4]);
	temp1[7]=costab[5]*(data[2]-data[5]);

	data[0]=(double)(temp1[0]+temp1[2]);
	data[1]=(double)(temp1[1]+temp1[3]);
	data[2]=(double)(costab[1]*(temp1[0]-temp1[2]));
	data[3]=(double)(costab[2]*(temp1[1]-temp1[3]));
	data[4]=(double)(temp1[4]+temp1[6]);
	data[5]=(double)(temp1[5]+temp1[7]);
	data[6]=(double)(costab[1]*(temp1[4]-temp1[6]));
	data[7]=(double)(costab[2]*(temp1[5]-temp1[7]));

	temp1[0]=data[0]+data[1];
	temp1[1]=costab[0]*(data[0]-data[1]);
	temp1[2]=0.5*(data[2]+data[3]);
	temp1[3]=costab[0]*(data[2]-data[3]);
	temp1[4]=data[4]+data[5];
	temp1[5]=costab[0]*(data[4]-data[5]);
	temp1[6]=0.5*(data[6]+data[7]);
	temp1[7]=costab[0]*(data[6]-data[7])-temp1[6];

	data[0]=(double)(0.7071068*temp1[0]);
	data[4]=(double)(temp1[1]);
	data[2]=(double)(temp1[2]);
	data[6]=(double)(temp1[3]-temp1[2]);
	data[1]=(double)(0.5*temp1[4]);
	data[3]=(double)(temp1[6]-data[1]);
	data[5]=(double)(temp1[5]-data[3]);
	data[7]=(double)(temp1[7]-data[5]);
}

void VideoFrameObservable::transform_block(double data[BLOCK_SIZE][BLOCK_SIZE],double quantable[BLOCK_SIZE][BLOCK_SIZE])
{
	int i,j;
	double y[BLOCK_SIZE],(*p)[BLOCK_SIZE];
	p=data;

	for(i=0;i<BLOCK_SIZE;i++)
	{
		for(j=0;j<BLOCK_SIZE;j++)
		{
			y[j]=data[j][i]-128;
		}
		d1dct (y);
		for(j=0;j<BLOCK_SIZE;j++)
		{
			data[j][i]=y[j];
		}
	}
	for(i=0;i<BLOCK_SIZE;i++)
	{
		d1dct(p[i]);
	}
	for(i=0;i<BLOCK_SIZE;i++)
	{
		for(j=0;j<BLOCK_SIZE;j++)
		{
			 data[i][j]=(int)(data[i][j]/quantable[i][j]);
		}
	}
}

void VideoFrameObservable::releaseform_block(double data[BLOCK_SIZE][BLOCK_SIZE],double iquantable[BLOCK_SIZE][BLOCK_SIZE])
{
	int i,j;
	double y[BLOCK_SIZE],(*p)[BLOCK_SIZE];
	p=data;

	for(i=0;i<BLOCK_SIZE;i++)
	{
		for(j=0;j<BLOCK_SIZE;j++)
		{
			data[i][j]*=iquantable[i][j];
		}
	}
	for(i=0;i<BLOCK_SIZE;i++)
	{
		d1idct(p[i]);
	}
	for(i=0;i<BLOCK_SIZE;i++)
	{
		for(j=0;j<BLOCK_SIZE;j++)
		{
			y[j]=data[j][i];
		}
		d1idct (y);
		for(j=0;j<BLOCK_SIZE;j++)
		{
			data[j][i]=y[j]+128;
		}
	}
}

void VideoFrameObservable::reduce()
{
	reduce(m_height, m_width);
	m_height /= BLOCK_SIZE;
	m_width	 /= BLOCK_SIZE;
	if (isCalibrated())
	{
		if(checkPixel(m_cursor_x, m_cursor_y))
		{
			m_pointRed = 0;
			m_pointGreen = 255;
			m_pointBlue = 0;

			moveCursor();

		} else {
			m_pointRed = 255;
			m_pointGreen = 0;
			m_pointBlue = 0;
		}
		/*m_virtCur->detectHandFromPoint(m_cursor_y,m_cursor_x);
		m_virtCur->analyseRadiusMinMax();
		m_virtCur->detectFingers();*/
	}
}

void VideoFrameObservable::reduce(int w, int h)
{
	h = h - h % BLOCK_SIZE;
	w = w - w % BLOCK_SIZE;
	int dw = m_width / BLOCK_SIZE;

	double data[3][BLOCK_SIZE][BLOCK_SIZE];

	for (int i=0;i<w;i+=BLOCK_SIZE)
	{
		for (int j=0;j<h;j+=BLOCK_SIZE)
		{
			for (int k=0;k<BLOCK_SIZE;k++)
			{
				for (int l=0;l<BLOCK_SIZE;l++)
				{
					for (int m=0;m<m_nchannels;m++)
					{
						data[m][k][l] = get_pointYCbCr(m, i+k,j+l);
					}
				}
			}

			double Y, Cb, Cr;

			for (int m=0;m<m_nchannels;m++)
			{
				transform_block(data[m],fullquantable);
				releaseform_block(data[m],fullquantable);
			}

			Y = 0.0, Cb = 0.0, Cr = 0.0;
			for (int k=0;k<BLOCK_SIZE;k++)
			{
				for (int l=0;l<BLOCK_SIZE;l++)
				{
					Y  += data[0][k][l];
					Cb += data[1][k][l];
					Cr += data[2][k][l];
				}
			}
			Y  =  Y /(BLOCK_SIZE*BLOCK_SIZE);
			Cb = Cb /(BLOCK_SIZE*BLOCK_SIZE);
			Cr = Cr /(BLOCK_SIZE*BLOCK_SIZE);

			int x = i/BLOCK_SIZE;
			int y = dw - j/BLOCK_SIZE-1;

			m_YCrCb[(x*dw+y)*3] = Y;
			m_YCrCb[(x*dw+y)*3+1] = Cb;
			m_YCrCb[(x*dw+y)*3+2] = Cr;

			Cb = Cb - 128;
			Cr = Cr - 128;
			set_pointBGR(2,x,y,Y + 1.402*Cr);
			set_pointBGR(1,x,y,Y - 0.34414*Cb - 0.71414*Cr);
			set_pointBGR(0,x,y,Y + 1.772*Cb);
		}
	}
}

void VideoFrameObservable::DrawPoint(int _x, int _y, int _r)
{
	for (int i = 0; i < m_height; i++)
	{
		int dx = (_x-i)*(_x-i);
		for (int j = 0; j < m_width; j++)
		{
			int dy = (_y-j)*(_y-j);
			if (sqrt(dx+dy) < _r)
			{
			     m_data[i * m_step + j * m_nchannels + 0] = m_pointBlue;
			     m_data[i * m_step + j * m_nchannels + 1] = m_pointGreen;
			     m_data[i * m_step + j * m_nchannels + 2] = m_pointRed;
			}
		}
	}
}

void VideoFrameObservable::DrawMarkers()
{
	/*if (isCalibrated())
	{
		m_virtCur->getPos(m_cursor_y, m_cursor_x);
	}*/
	DrawPoint(m_cursor_x, m_cursor_y, 5);
}

bool VideoFrameObservable::checkPixel(int _x, int _y)
{
	uchar Y =  get_pointYCbCr(0, _x,_y);
	uchar Cb = get_pointYCbCr(1, _x,_y);
	uchar Cr = get_pointYCbCr(2, _x,_y);

	return abs(Y-m_fingerY) < 20 && abs(Cb-m_fingerCb) < 20 &&
			abs(Cr-m_fingerCr) < 20;
}

void VideoFrameObservable::moveCursor()
{
	int x = m_cursor_x;
	int y = m_cursor_y;

	int i, j;

	for(i = x; i < m_width && checkPixel(i, y); i++) {}
	int down = i - x;

	for(i = x; i >= 0 && checkPixel(i, y); i--) {}
	int up = x - i;


	for(j = y; i < m_height && checkPixel(x, j); j++) {}
	int left = j - y;

	for(j = y; j >= 0 && checkPixel(x, j); j--) {}
	int right = y - j;

	int ddownleft;
	for(ddownleft = 0; x + ddownleft < m_width && y + ddownleft < m_height &&
	checkPixel(x+ddownleft, y+ddownleft); ddownleft++) { }

	int duprigth;
	for(duprigth = 0; x - duprigth > 0 && y-duprigth > 0 &&
	checkPixel(x - duprigth, y-duprigth); duprigth++) { }

	int dupleft;
	for(dupleft = 0; x - dupleft > 0 && y + dupleft < m_height &&
	checkPixel(x - dupleft, y+dupleft); dupleft++) { }

	int ddownrigth;
	for(ddownrigth = 0; x + ddownrigth < m_width  && y-ddownrigth > 0 &&
	checkPixel(x + ddownrigth, y-ddownrigth); ddownrigth++) { }

	int dx = (down-up)/2;// + (ddownleft-duprigth)/2 + (ddownrigth -dupleft)/2;
	int dy = (left-right)/2;// + (ddownleft-duprigth)/2 + (dupleft-ddownrigth)/2;

	if (dx != 0 && dy != 0)
	{
		m_cursor_x = m_cursor_x + dx;
		m_cursor_y = m_cursor_y + dy;
		notify(0, 10*dy, 10*dx);
	}
}

VideoFrameObservable::~VideoFrameObservable()
{
	delete m_virtCur;
	delete m_YCrCb;
}

